/*********************************************************************
*
* Software License Agreement (BSD License)
*
*  Copyright (c) 2010, Heriot-Watt University, UK.
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the Heriot-Watt University nor the names of
*     its contributors may be used to endorse or promote products
*     derived from this software without specific prior written
*     permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*
*  Author: Joel Cartwright
*
*********************************************************************/

#include <iostream>

#include <matchmaker/client.h>

#include <boost/foreach.hpp>

namespace matchmaker
{

// Declarations of static member variables.
boost::mutex Client::singletonMutex_;
bool Client::singletonActive_ = false;

// ------------------------------------------------------------
// Public methods
// ------------------------------------------------------------

Client::Client()
{
  active_ = false;

  if (!ros::isInitialized()) {
    std::cerr << "You must call ros::init before creating matchmaker Client." << std::endl;
    std::cerr << "If storing Client in a class member variable, consider ClientPtr instead." << std::endl;
    std::cerr << "See matchmaker doxygen mainpage for more information." << std::endl;
    return;
  }

  boost::unique_lock<boost::mutex> lock(singletonMutex_, boost::try_to_lock);
  if (!lock || singletonActive_) {
    std::cerr << "Only one matchmaker Client may be instantiated within a process." << std::endl;
    return;
  }

  nh_ = boost::make_shared<ros::NodeHandle>();

  waitForClock();

  advertPub_ = nh_->advertise<matchmaker_msgs::Advert>("soa/advert", 20,
      boost::bind(&Client::advertSubscriberConnected, this, _1));
  advertSendTimer_ = nh_->createTimer(ros::Duration(ADVERT_SEND_PERIOD), &Client::sendAdvertsCallback, this);

  clientUpdatePub_ = nh_->advertise<matchmaker_msgs::ClientUpdate>("soa/client_update", 1,
      boost::bind(&Client::serverUpdatePublisherConnected, this, _1));
  clientUpdateSendTimer_ = nh_->createTimer(ros::Duration(CLIENT_UPDATE_MSG_SEND_PERIOD),
      boost::bind(&Client::sendClientUpdateMsgCallback, this, _1));

  findServicesClient_ = nh_->serviceClient<matchmaker_msgs::FindServices>("soa/find", false /* persistent */);
  serviceStatusClient_ = nh_->serviceClient<matchmaker_msgs::GetServiceStatus>("soa/get_status", false /* persistent */);

  // This topic includes the node name as a prefix.
  std::string soa_status_topic = ros::this_node::getName() + "/soa/server_update";
  serverUpdateSub_ = nh_->subscribe<matchmaker_msgs::ServerUpdate>(soa_status_topic, 1, &Client::handleServerUpdateMsg, this);
  lastServiceStatusMsgTime_ = ros::Time::now();
  serviceStatusReceivedOnce_ = false;

  ROS_INFO_STREAM("Matchmaker client initialised, version " << getVersionString());

  active_ = true;
  singletonActive_ = true;
}


Client::~Client()
{
  shutdown();
}


void Client::shutdown()
{
  // This method should safely handle being called twice on the same
  // object, e.g. directly by user, then by destructor. Or by subclass
  // shutdown method, then by this class destructor.
  if (!active_) return;

  // Tell matchmaker we are not providing services any more.
  if (advertPub_) sendCancellationAdverts();

  // Clear local data structures.
  localServices_.clear();
  serviceMonitorMap_.clear();

  // Send empty service monitor message after clearing data structures.
  if (clientUpdatePub_ && serverUpdateSub_) {
    sendClientUpdateMsg();
  }

  // Stop timers.
  advertSendTimer_.stop();
  clientUpdateSendTimer_.stop();

  // Shutdown ROS communication classes.
  serviceStatusClient_.shutdown();
  advertPub_.shutdown();
  clientUpdatePub_.shutdown();
  findServicesClient_.shutdown();
  serverUpdateSub_.shutdown();

  // Shutdown the ROS node handle.
  nh_->shutdown();
  nh_.reset();

  active_ = false;

  // Don't need to try lock here, just go for it, as constructor
  // will only temporarily hold lock when we are active.
  boost::unique_lock<boost::mutex> lock(singletonMutex_);
  singletonActive_ = false;
}


bool Client::isConnected()
{
  if (!active_) return false;
  return findServicesClient_.exists();
}


std::string Client::getVersionString()
{
  static const std::string version(MATCHMAKER_VERSION);
  return version;
}


ServiceAdvertiser Client::advertiseService(const std::string& name, const std::string& topic)
{
  boost::lock_guard<boost::mutex> lock(classMutex_);

  std::string fullTopic = ros::names::resolve(topic, true /* apply remappings */);

  ServiceAdvertiser service(name, fullTopic,
      boost::bind(&Client::sendOneAdvert, this, _1));

  localServices_.push_back(service);

  return service;
}


ServiceAdvertiser Client::advertiseService(const std::string& name,
    const ros::Publisher& publisher)
{
  return advertiseService(name, publisher.getTopic());
}


ServiceAdvertiser Client::advertiseService(const std::string& name,
    const ros::Subscriber& subscriber)
{
  return advertiseService(name, subscriber.getTopic());
}


ServiceAdvertiser Client::advertiseService(const std::string& name,
    const image_transport::CameraPublisher& publisher)
{
  return advertiseService(name, publisher.getTopic());
}


ServiceDetailsPtr Client::findService(const std::string& name, bool onlyUsableNow)
{
  if (!active_) {
    std::cerr << "Matchmaker Client was incorrectly initialised." << std::endl;
    std::cerr << "Check errors output during construction." << std::endl;
    return ServiceDetailsPtr();
  }

  boost::lock_guard<boost::mutex> lock(classMutex_);

  if (name == "") {
    ROS_ERROR_STREAM("findService: Service name string cannot be empty.");
    return ServiceDetailsPtr();
  }

  matchmaker_msgs::FindServices srv;
  srv.request.query_type = matchmaker_msgs::FindServices::Request::QUERY_TYPE_PROVIDING;
  srv.request.services.push_back(name);
  std::string errorString;
  if (!doCheckedQueryForServices(srv, &errorString)) {
    ROS_ERROR_STREAM("findService error: " << errorString);
    return ServiceDetailsPtr();
  }

  L_ServiceDetails services;
  if (onlyUsableNow) {
    convertServicesFromQuery(srv, &services, boost::bind(&Client::testAdvertStatusUsable, _1));
  }
  else {
    convertServicesFromQuery(srv, &services);
  }


  if (services.size() == 0) {
    //ROS_INFO_STREAM("findService(" << name << ", availableOnly=" << availableOnly << "): No services found.");
    return ServiceDetailsPtr();
  }
  else {
    return services.front();
  }
}


ServiceStatus Client::getServiceStatus(const std::string& id)
{
  if (!active_) {
    std::cerr << "Matchmaker Client was incorrectly initialised." << std::endl;
    std::cerr << "Check errors output during construction." << std::endl;
    return ServiceStatus(ServiceStatus::REMOVED, "client init error");
  }
  boost::lock_guard<boost::mutex> lock(classMutex_);
  return getServiceStatusInternal(id);
}


ServiceMonitor Client::monitorService(const ServiceDetailsPtr& service,
    boost::function<void (const ServiceDetails& service)> statusCallback,
    float forcedCallbackInterval)
{
  if (!active_) {
    std::cerr << "Matchmaker Client was incorrectly initialised." << std::endl;
    std::cerr << "Check errors output during construction." << std::endl;
    return ServiceMonitor();
  }

  if (!service) {
    ROS_ERROR_STREAM("Client monitorService called with null ServiceDetailsPtr.");
    return ServiceMonitor();
  }

  const std::string& id = service->id;

  // Need to instantiate monitor and status objects here, because
  // we want to use a scoped lock below.
  ServiceMonitor monitor;

  // Block for scoped lock on big class mutex.
  {
    boost::lock_guard<boost::mutex> lock(classMutex_);

    if (serviceMonitorMap_.find(id) != serviceMonitorMap_.end()) {
      ROS_WARN_STREAM("A monitor already exists for service " << id << "; not creating another.");
      return ServiceMonitor();
    }

    monitor = ServiceMonitor(service, statusCallback);

    if (forcedCallbackInterval > 0)
    {
      ros::Timer timer = nh_->createTimer(ros::Duration(forcedCallbackInterval),
          boost::bind(&ServiceMonitor::performCallback, monitor));

      monitor.storeTimer(timer);
      ROS_DEBUG_STREAM("Created service monitor for " << id << " with callback and interval " << forcedCallbackInterval);
    }
    else
    {
      ROS_DEBUG_STREAM("Created service monitor for " << id << " with callback.");
    }

    // Add the monitor to the map.
    serviceMonitorMap_[monitor.getID()] = monitor;

    monitor.setStatus(getServiceStatusInternal(id));
  }

  // With the mutex unlocked, now safely perform the callback.
  // The user code could may call a method on this class that tries to
  // lock the mutex again.
  monitor.performCallback();

  scheduleSendClientUpdateMsg();

  return monitor;
}


ServiceMonitor Client::monitorService(const ServiceDetailsPtr& service)
{
  if (!active_) {
    std::cerr << "Matchmaker Client was incorrectly initialised." << std::endl;
    std::cerr << "Check errors output during construction." << std::endl;
    return ServiceMonitor();
  }
  boost::lock_guard<boost::mutex> lock(classMutex_);

  if (!service) {
    ROS_ERROR_STREAM("Client monitorService called with null ServiceDetailsPtr.");
    return ServiceMonitor();
  }

  const std::string& id = service->id;

  if (serviceMonitorMap_.find(id) != serviceMonitorMap_.end()) {
    ROS_WARN_STREAM("A monitor already exists for service " << id << "; not creating another.");
    return ServiceMonitor();
  }

  ServiceMonitor monitor(service);

  ROS_DEBUG_STREAM("Created service monitor for " << id);

  // Add the monitor to the map.
  serviceMonitorMap_[monitor.getID()] = monitor;

  // Get the service status, then set it.
  ServiceStatus status = getServiceStatusInternal(id);
  monitor.setStatus(status);
  monitor.performCallback();

  scheduleSendClientUpdateMsg();

  return monitor;
}


// ------------------------------------------------------------
// Protected internal methods
// ------------------------------------------------------------

void Client::waitForClock()
{
  bool use_sim_time;
  ros::param::param("/use_sim_time", use_sim_time, false);
  // If we're using simulation time, wait for the clock to be published before going any further.
  if (use_sim_time) {
    const ros::WallDuration diagPeriod(1);
    // Set diagTime in the future so diag won't be seen in most cases.
    ros::WallTime diagTime = ros::WallTime::now() + diagPeriod;
    while (ros::ok() && ros::Time::now().sec == 0) {
      if (ros::WallTime::now() > diagTime) {
        // Can't use throttled version of ROS_INFO because it will use sim time.
        ROS_INFO_STREAM("Waiting for first ROS simulation clock message.");
        diagTime += diagPeriod;
      }
      ros::WallDuration(0.01).sleep();
    }

    if (!ros::ok()) {
      ROS_WARN_STREAM("!ros::ok() whilst waiting for simulation clock.");
      return;
    }
  }
  else {
    if (ros::Time::now().sec == 0) {
      ROS_ERROR_STREAM("Param use_sim_time is not true, but time is zero!");
      exit(-1);
    }
  }
}


bool Client::doCheckedQueryForServices(matchmaker_msgs::FindServices & srv,
    std::string * errorString)
{
  if (!findServicesClient_.call(srv))
  {
    if (findServicesClient_.exists()) {
      *errorString = "Matchmaker query failed";
    }
    else {
      *errorString = "Matchmaker not available";
    }
    return false;
  }

  size_t expectedNumResultSets = srv.request.services.size();
  if (expectedNumResultSets == 0) {
    expectedNumResultSets = 1;
  }

  if (srv.response.num_results.size() != expectedNumResultSets)
  {
    std::stringstream ss;
    ss << "Response num_results size " << srv.response.num_results.size()
            << ", expected " << expectedNumResultSets;
    *errorString = ss.str();
    return false;
  }

  if (srv.response.adverts.size() != srv.response.provider_names.size())
  {
    std::stringstream ss;
    ss << "Response service array size " << srv.response.adverts.size()
            << " != provider_name array size " << srv.response.provider_names.size();
    *errorString = ss.str();
    return false;
  }

  size_t totalResults = 0;
  for (size_t i=0; i<srv.response.num_results.size(); ++i)
  {
    totalResults += srv.response.num_results[i];
  }

  if (srv.response.adverts.size() != totalResults)
  {
    std::stringstream ss;
    ss << "Response service array size " << srv.response.adverts.size()
            << ", expected " << totalResults;
    *errorString = ss.str();
    return false;
  }

  // Query response passes checks, it can be used now.
  return true;
}


void Client::convertServicesFromQuery(const matchmaker_msgs::FindServices & srv,
    L_ServiceDetails *services,
    boost::function<bool (const matchmaker_msgs::Advert & o)> includeFilter)
{
  //ROS_INFO("Services:");

  if (srv.response.adverts.size() == 0)
  {
    //ROS_INFO("  None");
    return;
  }

  // TODO: Possibly optimise by just comparing state of services, not everything.
  // The rest should stay the same, anyway.

  assert(srv.response.adverts.size() == srv.response.provider_names.size());

  std::vector<matchmaker_msgs::Advert>::const_iterator serviceIter;
  std::vector<std::string>::const_iterator providerIter = srv.response.provider_names.begin();
  int i = 0;
  // Note that for increments both serviceIter and serviceIdIter.
  // Assertion above ensure both have the same size.
  for (serviceIter = srv.response.adverts.begin();
      serviceIter != srv.response.adverts.end();
      serviceIter++, providerIter++)
  {
    if (!includeFilter(*serviceIter)) continue;

//    ROS_INFO("  %02d: type <%s>, status %d, name <%s>, topic <%s>, status_topic <%s>", i,
//        serviceIter->service.type.c_str(),
//        serviceIter->status,
//        serviceIter->service.service_name.c_str(),
//        serviceIter->topic.c_str(),
//        serviceIter->status_topic.c_str());

    ServiceDetailsPtr service = boost::shared_ptr<ServiceDetails>(new ServiceDetails(
        serviceIter->service_name,
        serviceIter->topic,
        ServiceStatus((ServiceStatus::Enum)serviceIter->status, serviceIter->extra_status),
        *providerIter));

    services->push_back(service);
    i++;
  }

  if (i == 0)
  {
    //ROS_INFO("  None matching filter");
    return;
  }
}


ServiceStatus Client::getServiceStatusInternal(const std::string& id)
{
  // Check if we have a (valid) monitor for this ID.
  M_StringToServiceMonitor::iterator iter = serviceMonitorMap_.find(id);
  if (iter != serviceMonitorMap_.end() && (iter->second)) {
    //ROS_INFO_STREAM("getServiceStatus: Returning cached status for service " << id);
    return iter->second.getStatus();
  }

  // TODO: Future enhancement to auto-create monitors for IDs that are queried > 1 Hz

  // Now we don't have a usable status cached, so we have to query for it.

  matchmaker_msgs::GetServiceStatus srv;
  srv.request.ids.push_back(id);

  if (!serviceStatusClient_.call(srv)) {
    ROS_WARN_STREAM_THROTTLE(1.0, "getServiceStatus call failed!");
    return ServiceStatus(ServiceStatus::REMOVED, "get status failed");
  }

  if (srv.response.status.size() != 1) {
    ROS_ERROR_STREAM("getServiceStatus for single ID, invalid response status size " << srv.response.status.size());
    return ServiceStatus(ServiceStatus::REMOVED, "bad matchmaker response");
  }

  if (srv.response.extra_status.size() != 1) {
    ROS_ERROR_STREAM("getServiceStatus for single ID, invalid response extra status size " << srv.response.extra_status.size());
    return ServiceStatus(ServiceStatus::REMOVED, "bad matchmaker response");
  }

  //ROS_INFO_STREAM("getServiceStatus: Returning polled status for service " << id);

  return ServiceStatus((ServiceStatus::Enum)srv.response.status[0], srv.response.extra_status[0]);
}


void Client::sendAdvertsCallback(const ros::TimerEvent& event)
{
  boost::lock_guard<boost::mutex> lock(classMutex_);

  sendAllAdverts();
}


void Client::sendAllAdverts()
{
  //ROS_INFO_STREAM("Sending all adverts.");

  L_ServiceAdvertiser::iterator serviceIter;
  for (serviceIter = localServices_.begin(); serviceIter != localServices_.end(); /* no ++ */)
  {
    if (serviceIter->getNumCopies() == 1)
    {
      ServiceAdvertiser& service = *serviceIter;
      if (service.getStatusTime().isZero()) {
        ROS_WARN_STREAM("Expiring advertisement that never had status set for service: " << service.getName());
        ROS_WARN_STREAM("Did you not store the object returned by advertiseService()?");
      }
      else {
        ROS_INFO_STREAM("Expiring advertisement for service: " << service.getName());
        // setStatus call will also publish.
        service.setStatus(ServiceStatus::REMOVED);
      }
      serviceIter = localServices_.erase(serviceIter);
    }
    else
    {
      // Dereference iterator to get ServiceAdvertiser, then take address to get ServiceAdvertiser*
      sendOneAdvert(&(*serviceIter));

      // Remember to increment the iterator; has to be done here because of potential erase above.
      serviceIter++;
    }
  }
}


void Client::sendOneAdvert(ServiceAdvertiser* service)
{
  // Acquire lock to publish.
  boost::lock_guard<boost::mutex> lock(advertPubMutex_);

  // Check stamp is non-zero to see if service has been set.
  const ros::Duration creationGraceDuration(ADVERT_CREATION_GRACE_PERIOD);
  ros::Time creationGraceTime = ros::Time::now() - creationGraceDuration;

  if (service->getStatusTime().isZero())
  {
    if (service->getCreationTime() < creationGraceTime)
    {
      ROS_WARN_STREAM("Status still not set after " << ADVERT_CREATION_GRACE_PERIOD << " sec for service: " << service->getName());
    }
    return;
  }

  matchmaker_msgs::Advert* msg = service->getAdvertMsg();

  // Set the timestamp and publish the mesage.
  msg->stamp = ros::Time::now();
  advertPub_.publish(*msg);
}


void Client::sendCancellationAdverts()
{
  L_ServiceAdvertiser::iterator serviceIter;
  BOOST_FOREACH(ServiceAdvertiser& service, localServices_) {
    // setStatus call will also publish.
    ROS_INFO_STREAM("Sending cancellation advert for: " << service.getName());
    service.setStatus(ServiceStatus::REMOVED);
  }
}


void Client::advertSubscriberConnected(const ros::SingleSubscriberPublisher& publisher)
{
  boost::lock_guard<boost::mutex> lock(classMutex_);

  ROS_DEBUG_STREAM("Matchmaker server connected to our advert stream.");
  sendAllAdverts();
}


void Client::serverUpdatePublisherConnected(const ros::SingleSubscriberPublisher& publisher)
{
  boost::lock_guard<boost::mutex> lock(classMutex_);

  ROS_DEBUG_STREAM("Matchmaker server connected to our monitor msg stream.");
  sendClientUpdateMsg();
}


void Client::scheduleSendClientUpdateMsg()
{
  // Schedule a one-shot timer to send the update msg to the matchmaker.
  // Setting a timer rather than calling the method directly
  // allows the message to be sent once after creating several
  // monitors or queries, rather than wastefully after each one.

  // Only create the timer if one isn't already pending, otherwise ROS gives us a
  // warning for destroying the old one quickly after creation.
  if (!clientUpdateSendOneshotTimer_.isValid()) {
    // Timer with tiny duration to expire nearly instantly.
    clientUpdateSendOneshotTimer_ = nh_->createTimer(ros::Duration(SCHEDULED_SEND_TIMEOUT),
        &Client::sendClientUpdateMsgCallback, this, true /* oneshot */);
  }
}


void Client::sendClientUpdateMsgCallback(const ros::TimerEvent& event)
{
  boost::lock_guard<boost::mutex> lock(classMutex_);

  sendClientUpdateMsg();
}


void Client::sendClientUpdateMsg()
{
  //ROS_INFO_STREAM("sendServiceMonitorMsgs");

  matchmaker_msgs::ClientUpdate msg;
  msg.status_topic = serverUpdateSub_.getTopic();

  fillClientUpdateMonitors(msg);

  fillClientUpdateQueries(msg);

  // Set the timestamp and publish the mesage.
  msg.stamp = ros::Time::now();
  clientUpdatePub_.publish(msg);

  // Now check if status message reception has timed out. Should really have its own method.

  const ros::Duration serviceStatusMsgTimeout(SERVICE_STATUS_MSG_TIMEOUT);
  ros::Duration timeSinceLastServiceStatus = ros::Time::now() - lastServiceStatusMsgTime_;
  if (timeSinceLastServiceStatus > serviceStatusMsgTimeout)
  {
    if (!serviceStatusReceivedOnce_) {
      ROS_WARN_STREAM_THROTTLE(1, "No matchmaker service status since start " << timeSinceLastServiceStatus.toSec() << " sec ago!");
      ROS_WARN_STREAM_THROTTLE(1, "Is the matchmaker server running?");
    }
    else {
      ROS_WARN_STREAM_THROTTLE(1, "Last matchmaker service status " << timeSinceLastServiceStatus.toSec() << " sec ago!");
    }

    // TODO: Should we take action here, such as put monitored services in REMOVED state?
  }
}


void Client::fillClientUpdateMonitors(matchmaker_msgs::ClientUpdate& msg)
{
  // Iterate through the service monitors.
  // Add those we're keeping to the message, and those we're not to eraseList.
  std::list<std::string> expiredList;
  BOOST_FOREACH(M_StringToServiceMonitor::value_type& pair, serviceMonitorMap_)
  {
    if (pair.second.getNumCopies() == 1)
    {
      expiredList.push_back(pair.first);
    }
    else
    {
      msg.ids.push_back(pair.first);
    }
  }

  // Erase expired monitors.
  BOOST_FOREACH(const std::string& id, expiredList)
  {
    ROS_DEBUG_STREAM("Expiring ServiceMonitor for: " << id);
    // Erasing from the map will cause the last smart pointer to be destroyed, cleaning up the object.
    serviceMonitorMap_.erase(id);
  }
}


void Client::fillClientUpdateQueries(matchmaker_msgs::ClientUpdate& msg)
{
  // Iterate through the service queries.
  // Add those we're keeping to the message, and those we're not to eraseList.
  std::list<std::string> expiredList;
  BOOST_FOREACH(M_StringToServiceQuery::value_type& pair, serviceQueryMap_)
  {
    if (pair.second.getNumCopies() == 1)
    {
      expiredList.push_back(pair.first);
    }
    else
    {
      msg.names.push_back(pair.first);
    }
  }

  // Erase expired queries.
  BOOST_FOREACH(const std::string& name, expiredList)
  {
    ROS_DEBUG_STREAM("Expiring ServiceQuery for: " << name);
    // Erasing from the map will cause the last smart pointer to be destroyed, cleaning up the object.
    serviceQueryMap_.erase(name);
  }
}


void Client::handleServerUpdateMsg(const matchmaker_msgs::ServerUpdateConstPtr& msg)
{
  L_ServiceMonitor monitorCallbackList;
  L_ServiceQuery queryCallbackList;

  {
    boost::lock_guard<boost::mutex> lock(classMutex_);

    //ROS_INFO_STREAM("handleServiceStatusMsg: Num ids " << msg->ids.size());

    handleMonitorUpdates(msg, monitorCallbackList);

    handleQueryUpdates(msg, queryCallbackList);

    lastServiceStatusMsgTime_ = ros::Time::now();
    serviceStatusReceivedOnce_ = true;
  }

  // Now outside the Client lock, we can perform the callbacks.

  BOOST_FOREACH(ServiceMonitor& monitor, monitorCallbackList) {
    monitor.performCallback();
  }

  BOOST_FOREACH(ServiceQuery& query, queryCallbackList) {
    query.performCallback();
  }
}


void Client::handleMonitorUpdates(const matchmaker_msgs::ServerUpdateConstPtr& msg,
    L_ServiceMonitor& monitorCallbackList)
{
  if (msg->ids.size() != msg->status.size()) {
    ROS_ERROR_STREAM("handleMonitorUpdates: ids size " << msg->ids.size() << " != status size " << msg->status.size());
    return;
  }

  if (msg->ids.size() != msg->extra_status.size()) {
    ROS_ERROR_STREAM("handleMonitorUpdates: ids size " << msg->ids.size() << " != extra_status size " << msg->extra_status.size());
    return;
  }

  // Add all monitored IDs we are expecting to the stale set.
  std::set<std::string> staleSet;
  BOOST_FOREACH(M_StringToServiceMonitor::value_type& pair, serviceMonitorMap_)
  {
    staleSet.insert(pair.first);
  }

  std::list<std::string> expiredList;

  // Iterate through the (id, status) results, removing those received from
  // the stale list.
  std::map<std::string, ServiceMonitor>::iterator monitorIter;
  int index = -1;
  BOOST_FOREACH(const std::string& id, msg->ids)
  {
    // Increment the index (starts at -1, so we get 0)
    index++;

    // Skip the rest if we don't have a matching monitor.
    monitorIter = serviceMonitorMap_.find(id);
    if (monitorIter == serviceMonitorMap_.end()) {
       ROS_DEBUG_STREAM("handleMonitorUpdates: Received an ID we are not monitoring: " << id);
       continue;
    }

    staleSet.erase(id);

    // If user has expired the monitor, add to the expired list and skip.
    if (monitorIter->second.getNumCopies() == 1) {
      expiredList.push_back(monitorIter->first);
      continue;
    }

    ServiceMonitor& monitor = monitorIter->second;
    ServiceStatus status((ServiceStatus::Enum)msg->status[index], msg->extra_status[index]);
    if (monitor.getStatus() != status || monitor.getStatusChangeTime().isZero()) {
      ROS_DEBUG_STREAM("Status of " << id << " new or changed, scheduling a monitor callback.");
      monitorCallbackList.push_back(monitor);
    }

    //ROS_INFO_STREAM("handleServiceStatusMsg: Updating status of service " << id << " to " << status.toString());
    monitor.setStatus(status);
  }

  BOOST_FOREACH(const std::string& id, staleSet)
  {
    ServiceMonitor& monitor = serviceMonitorMap_[id];
    // Complain about missing status and set monitor to removed, but only if the monitor was
    // created sufficiently long ago to ensure the matchmaker knows we are monitoring it,
    // and that this isn't a status message that was sent before the matchmaker knew.
    //
    // The initial status query on monitor creation and the instant on-status-change updates
    // from the matchmaker means the client should have the most up to date status.

    if (ros::Time::now() - monitor.getCreationTime() > ros::Duration(SERVICE_STATUS_MSG_TIMEOUT)) {
      ROS_WARN_STREAM("handleMonitorUpdates: Missing status for monitored service " << id);

      monitor.setStatus(ServiceStatus::REMOVED);

      if (monitor.getStatus() != ServiceStatus::REMOVED || monitor.getStatusChangeTime().isZero()) {
        ROS_DEBUG_STREAM("Status of " << id << " new or changed, scheduling a monitor callback.");
        monitorCallbackList.push_back(monitor);
      }
    }
  }

  // Erase expired monitors.
  BOOST_FOREACH(const std::string& id, expiredList)
  {
    ROS_DEBUG_STREAM("Expiring ServiceMonitor for: " << id);
    // Erasing from the map will cause the last smart pointer to be destroyed, cleaning up the object.
    serviceMonitorMap_.erase(id);
  }
}


void Client::handleQueryUpdates(const matchmaker_msgs::ServerUpdateConstPtr& msg,
    L_ServiceQuery& queryCallbackList)
{
  unsigned totalResults = 0;
  BOOST_FOREACH(int numResults, msg->num_results) {
    totalResults += numResults;
  }
  if (totalResults != msg->adverts.size()) {
    ROS_ERROR_STREAM("handleQueryUpdates: sum of num_results " << totalResults << " != adverts size " << msg->adverts.size());
    return;
  }
  if (totalResults != msg->provider_names.size()) {
    ROS_ERROR_STREAM("handleQueryUpdates: sum of num_results " << totalResults << " != provider_names size " << msg->provider_names.size());
    return;
  }

  // Add all queried names we are expecting to the stale set.
  std::set<std::string> staleSet;
  BOOST_FOREACH(M_StringToServiceQuery::value_type& pair, serviceQueryMap_)
  {
    staleSet.insert(pair.first);
  }

  std::list<std::string> expiredList;

  // Iterate through the results, removing those received from
  // the stale list.
  std::map<std::string, ServiceQuery>::iterator queryIter;
  int queryIndex = -1;
  int serviceIndex = 0;
  BOOST_FOREACH(const std::string& serviceName, msg->names)
  {
    // Increment the index (starts at -1, so we get 0)
    queryIndex++;

    // Skip results if we don't have a matching query.
    queryIter = serviceQueryMap_.find(serviceName);
    if (queryIter == serviceQueryMap_.end()) {
       ROS_DEBUG_STREAM("handleQueryUpdates: Received results for query we don't have: '" << serviceName << "'");
       continue;
    }

    staleSet.erase(serviceName);

    // If user has expired the query, add to the expired list and skip.
    if (queryIter->second.getNumCopies() == 1) {
      expiredList.push_back(queryIter->first);
      continue;
    }

    ServiceQuery& query = queryIter->second;

    L_ServiceDetails& serviceList = query.impl_->results_;
    // Create a copy of the current service list.
    L_ServiceDetails oldServiceList(serviceList);
    serviceList.clear();

    std::vector<matchmaker_msgs::Advert>::const_iterator serviceIter;
    int numResults = msg->num_results[queryIndex];
    for (int i=0; i<numResults; ++i) {

      const matchmaker_msgs::Advert& advert = msg->adverts[serviceIndex];
      const std::string& provider = msg->provider_names[serviceIndex];
      ServiceDetailsPtr service = boost::shared_ptr<ServiceDetails>(new ServiceDetails(
          advert.service_name,
          advert.topic,
          ServiceStatus((ServiceStatus::Enum)advert.status, advert.extra_status),
          provider));

      serviceList.push_back(service);

      serviceIndex++;
    }

    bool resultsChanged = false;
    if (oldServiceList.size() != serviceList.size()) {
      resultsChanged = true;
    }
    else {
      L_ServiceDetails::iterator newIter, oldIter;
      for (newIter = serviceList.begin(), oldIter = oldServiceList.begin();
          newIter != serviceList.end(); newIter++, oldIter++) {
        if (!(*newIter)->hasSameContent( *(oldIter->get()) )) {
          resultsChanged = true;
        }
      }
    }

    if (resultsChanged) {
      ROS_DEBUG_STREAM("Different results for query of '" << query.getName() << "'; scheduling callback.");
      query.setResultsChangeTime(msg->stamp);
      queryCallbackList.push_back(query);
    }
  }

  BOOST_FOREACH(const std::string& serviceName, staleSet)
  {
    ServiceQuery query = serviceQueryMap_[serviceName];
    // Complain about missing results, but only if the monitor was created sufficiently
    // long ago to ensure the matchmaker knows we are monitoring it, and that this
    // isn't a status message that was sent before the matchmaker knew.
    //
    // The initial 'find' on query creation and the instant on-status-change updates
    // from the matchmaker means the client should have the most up to date status.

    if (ros::Time::now() - query.getCreationTime() > ros::Duration(SERVICE_STATUS_MSG_TIMEOUT)) {
      ROS_WARN_STREAM("handleQueryUpdates: Missing results for queried service '" << serviceName << "'");
      if (query.impl_->results_.size() > 0) {
        query.impl_->results_.clear();
        queryCallbackList.push_back(query);
      }
    }
  }

}


bool Client::testAdvertStatusUsable(const matchmaker_msgs::Advert & o)
{
  if (o.status == matchmaker_msgs::Advert::STATUS_AVAILABLE
      || o.status == matchmaker_msgs::Advert::STATUS_ACTIVE /* not ACTIVE_BUSY */)
  {
    return true;
  }
  return false;
}


} // namespace
